#include "Character.h"
#include <cmath>
#include <iostream>
#define PI 3.14159265
#define BONE_SIZE 0.5

Character::Character(){
	//create arms and legs
	center = Vector3f(-0.1,0.7,0);
	Vector3f leftArm_origin(center(0) - 1.5*BONE_SIZE, center(1), center(2));
	leftArm.initLink(leftArm_origin, PI/2, 11*PI/8, BONE_SIZE, PI/2, 11*PI/8);
	leftArm.addLink(0, 0, BONE_SIZE, PI/2, 11*PI/8);
	leftArm.addLink(0, 0, BONE_SIZE, PI/2, 11*PI/8);

	Vector3f rightArm_origin(center(0) + 1.7*BONE_SIZE, center(1), center(2));
	rightArm.initLink(rightArm_origin, PI/2, -3*PI/8, BONE_SIZE, PI/2, -3*PI/8);
	rightArm.addLink(0, 0, BONE_SIZE, PI/2, -3*PI/8);
	rightArm.addLink(0, 0, BONE_SIZE, PI/2, -3*PI/8);


	Vector3f leftLeg_origin(center(0)- 0.5* BONE_SIZE , center(1) - 2*BONE_SIZE, center(2));
	leftLeg.initLink(leftLeg_origin, PI/2, 23*PI/16, BONE_SIZE,PI/2, 23*PI/16);
	leftLeg.addLink(0, 0, 1.5*BONE_SIZE, PI/2, 23*PI/16);
	leftLeg.addLink(0, 0, BONE_SIZE, PI/2, 23*PI/16);

	Vector3f rightLeg_origin(center(0)+ 0.7* BONE_SIZE, center(1) - 2*BONE_SIZE, center(2));
	rightLeg.initLink(rightLeg_origin, PI/2, -7 *PI/16, BONE_SIZE, PI/2, -7 *PI/16);
	rightLeg.addLink(0, 0, 1.5*BONE_SIZE, PI/2, -7 *PI/16);
	rightLeg.addLink(0, 0, BONE_SIZE, PI/2, -7 *PI/16);
	//set frame to 0
	frame =0;

	//create body
	Link* head = new Link(center, 0, 0, BONE_SIZE, PI/2, PI/2); 
	Link* spine = new Link(center, 0, 0, BONE_SIZE*2, PI/2, 3*PI/2);

	//create shoulder
	Link* left_shoulder = new Link(center, 0, 0,  1.5*BONE_SIZE, PI/2, PI); 
	Link* right_shoulder = new Link(center, 0, 0,  1.7*BONE_SIZE, PI/2, 0); 
 


	Vector3f lefthip_origin(center(0) , center(1) - 2*BONE_SIZE, center(2));
	Vector3f righthip_origin(center(0)  , center(1) + 2*BONE_SIZE, center(2));

	//create hip
	Link* left_hip = new Link(lefthip_origin, 0, 0, 0.5* BONE_SIZE, PI/2, PI);
	Link* right_hip = new Link(righthip_origin, 0,0, 0.5* BONE_SIZE, PI/2, 0);


	body.push_back(head);
	body.push_back(spine);
	body.push_back(left_shoulder);
	body.push_back(right_shoulder);
	body.push_back(left_hip);
	body.push_back(right_hip);

	body.push_back(leftArm.links[0]);
	body.push_back(leftArm.links[1]);
	body.push_back(leftArm.links[2]);

	body.push_back(rightArm.links[0]);
	body.push_back(rightArm.links[1]);
	body.push_back(rightArm.links[2]);

	body.push_back(leftLeg.links[0]);
	body.push_back(leftLeg.links[1]);
	body.push_back(leftLeg.links[2]);

	body.push_back(rightLeg.links[0]);
	body.push_back(rightLeg.links[1]);
	body.push_back(rightLeg.links[2]);

}

void Character::test(Vector3f v){
	drawBody();
	leftArm.calculate(v);
	leftArm.display();
	rightArm.display();
	leftLeg.display();
	rightLeg.display();
}
void Character::display(){
	drawBody();
	leftArm.display();
	rightArm.display();
	leftLeg.display();
	rightLeg.display();
}
void Character::walk(float scalar){
	Vector3f leftArmPos;
	Vector3f rightArmPos;
	Vector3f leftLegPos;
	Vector3f rightLegPos;

	switch(frame){
		case 0:
		case 2:
			leftArmPos = 5*Vector3f( -0.24, -0.08, 0);
			rightArmPos =5* Vector3f( 0.24, -0.08, 0);
			leftLegPos = 5*Vector3f( -0.18, -0.3, 0);
			rightLegPos =5* Vector3f( 0.18, -0.3, 0);
		break;
		case 1:
			leftArmPos  = 5*Vector3f( -0.24, -0.08, 0.2);
			rightArmPos = 5*Vector3f( 0.24, -0.08, -0.2);
			leftLegPos  = 5*Vector3f( -0.18, -0.31, -0.2);
			rightLegPos = 5*Vector3f( 0.18, -0.31, 0.2);
			
			break;
		case 3:			
			leftArmPos  = 5*Vector3f( -0.24, -0.08, -0.2);
			rightArmPos = 5*Vector3f( 0.24, -0.08, 0.2);
			leftLegPos  = 5*Vector3f( -0.18, -0.3, 0.2);
			rightLegPos = 5*Vector3f( 0.18, -0.3, -0.2);
			break;	
	}
	
	
	leftArm.calculate(leftArmPos/scalar);
	
	rightArm.calculate(rightArmPos/scalar);
	
	leftLeg.calculate(leftLegPos/scalar);
	
	rightLeg.calculate(rightLegPos/scalar);

	frame++;
	//cout<<frame<<endl;

	frame = frame%4;
}
void Character::drawBody(){
	
	glColor3f(0,0,1);
	//draw center
	drawCube(center(0), center(1), center(2)); 
	//draw head
	drawCube(center(0), center(1) + BONE_SIZE, center(2));
	//draw spine
	drawCube(center(0), center(1) - 2*BONE_SIZE, center(2));
	//draw left shoulder
	drawCube(center(0)- 1.5*BONE_SIZE, center(1), center(2));
	//draw right shoulder
	drawCube(center(0)+ 1.7*BONE_SIZE, center(1), center(2));


	//draw left hip
	drawCube(center(0)- 0.5* BONE_SIZE , center(1) - 2*BONE_SIZE, center(2));
	//drwa right hip	
	drawCube(center(0)+ 0.7* BONE_SIZE, center(1) - 2*BONE_SIZE, center(2));


	glColor3f(1, 1, 1);

	glBegin(GL_LINES);
	glVertex3f(center(0), center(1), center(2)); 
	glVertex3f(center(0), center(1) + BONE_SIZE, center(2));
	glEnd();

	
	glBegin(GL_LINES);
	glVertex3f(center(0), center(1), center(2)); 
	glVertex3f(center(0), center(1) - 2*BONE_SIZE, center(2));
	glEnd();
		
	glBegin(GL_LINES);
	glVertex3f(center(0), center(1), center(2)); 
	glVertex3f(center(0) - 1.5*BONE_SIZE, center(1), center(2));
	glEnd();
	
	glBegin(GL_LINES);
	glVertex3f(center(0), center(1), center(2)); 
	glVertex3f(center(0) + 1.7*BONE_SIZE, center(1) , center(2));
	glEnd();

	glBegin(GL_LINES);
	glVertex3f(center(0), center(1) - 2*BONE_SIZE, center(2)); 
	glVertex3f(center(0)- 0.5* BONE_SIZE , center(1) - 2*BONE_SIZE, center(2));
	glEnd();

	glBegin(GL_LINES);
	glVertex3f(center(0), center(1) - 2*BONE_SIZE, center(2)); 
	glVertex3f(center(0)+ 0.7* BONE_SIZE, center(1) - 2*BONE_SIZE, center(2));
	glEnd();


}

void Character::drawCube(float x , float y , float z){
	 glBegin(GL_QUADS);		// Draw The Cube Using quads
 
    glVertex3f( x + 0.02f, y + 0.02f, z - 0.02f);	// Top Right Of The Quad (Top)
    glVertex3f( x - 0.02f, y + 0.02f, z - 0.02f);	// Top Left Of The Quad (Top)
    glVertex3f( x - 0.02f, y + 0.02f, z + 0.02f);	// Bottom Left Of The Quad (Top)
    glVertex3f( x + 0.02f, y + 0.02f, z + 0.02f);	// Bottom Right Of The Quad (Top)

    glVertex3f( x + 0.02f, y - 0.02f, z + 0.02f);	// Top Right Of The Quad (Bottom)
    glVertex3f( x - 0.02f, y - 0.02f, z + 0.02f);	// Top Left Of The Quad (Bottom)
    glVertex3f( x - 0.02f, y - 0.02f, z - 0.02f);	// Bottom Left Of The Quad (Bottom)
    glVertex3f( x + 0.02f, y - 0.02f, z - 0.02f);	// Bottom Right Of The Quad (Bottom)

    glVertex3f( x + 0.02f, y + 0.02f, z + 0.02f);	// Top Right Of The Quad (Front)
    glVertex3f( x - 0.02f, y + 0.02f, z + 0.02f);	// Top Left Of The Quad (Front)
    glVertex3f( x - 0.02f, y - 0.02f, z + 0.02f);	// Bottom Left Of The Quad (Front)
    glVertex3f( x + 0.02f, y - 0.02f, z + 0.02f);	// Bottom Right Of The Quad (Front)
 
    glVertex3f( x + 0.02f, y - 0.02f, z - 0.02f);	// Top Right Of The Quad (Back)
    glVertex3f( x - 0.02f, y - 0.02f, z - 0.02f);	// Top Left Of The Quad (Back)
    glVertex3f( x - 0.02f, y + 0.02f, z - 0.02f);	// Bottom Left Of The Quad (Back)
    glVertex3f( x + 0.02f, y + 0.02f, z - 0.02f);	// Bottom Right Of The Quad (Back)
 
    glVertex3f( x - 0.02f, y + 0.02f, z + 0.02f);	// Top Right Of The Quad (Left)
    glVertex3f( x - 0.02f, y + 0.02f, z - 0.02f);	// Top Left Of The Quad (Left)
    glVertex3f( x - 0.02f, y - 0.02f, z - 0.02f);	// Bottom Left Of The Quad (Left)
    glVertex3f( x - 0.02f, y - 0.02f, z + 0.02f);	// Bottom Right Of The Quad (Left)
 
    glVertex3f( x + 0.02f, y + 0.02f, z - 0.02f);	// Top Right Of The Quad (Right)
    glVertex3f( x + 0.02f, y + 0.02f, z + 0.02f);	// Top Left Of The Quad (Right)
    glVertex3f( x + 0.02f, y - 0.02f, z + 0.02f);	// Bottom Left Of The Quad (Right)
    glVertex3f( x + 0.02f, y - 0.02f, z - 0.02f);	// Bottom Right Of The Quad (Right)
    glEnd();			// End Drawing The Cube

}